#include "rtps/storages/SimpleHistoryCache.h"


static void
_shc_incrementIterator(struct rtps_SimpleHistoryCache *shc, uint16_t *iterator)
{
    ++(*iterator);
    if (*iterator >= sizeof(shc->m_buffer)/sizeof(shc->m_buffer[0])) {
        *iterator = 0;
    }
}

static void
_shc_incrementTail(struct rtps_SimpleHistoryCache *shc)
{
    _shc_incrementIterator(shc, &(shc->m_tail));
}

static void
_shc_incrementHead(struct rtps_SimpleHistoryCache *shc)
{
    _shc_incrementIterator(shc, &(shc->m_head));
    if (shc->m_head == shc->m_tail) {
        // Move without check
        _shc_incrementTail(shc); // drop one
    }
}

struct rtps_SimpleHistoryCache *
rtps_SimpleHistoryCache_init(SequenceNumber_t lastUsed)
{
    struct rtps_SimpleHistoryCache *shc =
        (struct rtps_SimpleHistoryCache *)calloc(1, sizeof(struct rtps_SimpleHistoryCache));

    if (shc == NULL)
        return NULL;

    shc->m_lastUsedSequenceNumber = lastUsed;

    for(uint32_t i=0; i<sizeof(shc->m_buffer)/sizeof(shc->m_buffer[0]); i++)
        shc->m_buffer[i] = rtps_CacheChange_init();
    
    return shc;
}

void
rtps_SimpleHistoryCache_deinit(struct rtps_SimpleHistoryCache *shc)
{
    if (shc == NULL)
        return;

    for(uint32_t i=0; i<sizeof(shc->m_buffer)/sizeof(shc->m_buffer[0]); i++)
        rtps_CacheChange_deinit(shc->m_buffer[i]);

    free(shc);
}

bool
rtps_SimpleHistoryCache_isFull(struct rtps_SimpleHistoryCache *shc)
{
    uint16_t it = shc->m_head;

    _shc_incrementIterator(shc, &it);
    if(it == shc->m_tail)
        return true;
    return false;
}

SequenceNumber_t
rtps_SimpleHistoryCache_getSeqNumMin(struct rtps_SimpleHistoryCache *shc)
{
    if (shc->m_head == shc->m_tail) {
        return SEQUENCENUMBER_UNKNOWN;
    } else {
        return shc->m_buffer[shc->m_tail]->sequenceNumber;
    }
}

SequenceNumber_t
rtps_SimpleHistoryCache_getSeqNumMax(struct rtps_SimpleHistoryCache *shc)
{
    if (shc->m_head == shc->m_tail) {
        return SEQUENCENUMBER_UNKNOWN;
    } else {
        return shc->m_lastUsedSequenceNumber;
    }
}

struct rtps_CacheChange *
rtps_SimpleHistoryCache_addChange(struct rtps_SimpleHistoryCache *shc, const uint8_t *data, DataSize_t size)
{
    struct rtps_CacheChange *place = shc->m_buffer[shc->m_head];
    _shc_incrementHead(shc);

    place->kind = ALIVE;
    rtps_PBufWrapper_reserve(place->data, size);
    rtps_PBufWrapper_append(place->data, data, size);

    rtps_SequenceNumber_t_operator_increase(&(shc->m_lastUsedSequenceNumber));
    place->sequenceNumber = shc->m_lastUsedSequenceNumber;
    return place;
}

void
rtps_SimpleHistoryCache_dropOldest(struct rtps_SimpleHistoryCache * shc)
{
    rtps_SimpleHistoryCache_removeUntilIncl(shc, rtps_SimpleHistoryCache_getSeqNumMin(shc));
}

void
rtps_SimpleHistoryCache_removeUntilIncl(struct rtps_SimpleHistoryCache *shc, SequenceNumber_t sn)
{
    SequenceNumber_t snMax = rtps_SimpleHistoryCache_getSeqNumMax(shc);

    if (shc->m_head == shc->m_tail) {
        return;
    }

    if (rtps_SequenceNumber_t_operator_lessequal(&snMax,&sn)) { // We won't overrun head
        shc->m_head = shc->m_tail;
        return;
    }

    while (rtps_SequenceNumber_t_operator_lessequal(&(shc->m_buffer[shc->m_tail]->sequenceNumber), &sn)) {
        _shc_incrementTail(shc);
    }
}

struct rtps_CacheChange *
rtps_SimpleHistoryCache_getChangeBySN(struct rtps_SimpleHistoryCache * shc, SequenceNumber_t sn)
{
    SequenceNumber_t snMax = rtps_SimpleHistoryCache_getSeqNumMax(shc);
    SequenceNumber_t minSN = rtps_SimpleHistoryCache_getSeqNumMin(shc);
    // if (sn < minSN || rtps_SimpleHistoryCache_getSeqNumMax(shc) < sn) {
    if (rtps_SequenceNumber_t_operator_less(&sn, &minSN) || 
        rtps_SequenceNumber_t_operator_less(&snMax, &sn)) {   
        return NULL;
    }

    uint16_t pos = shc->m_tail + (uint16_t)(sn.low - minSN.low);

    // Diff is smaller than the size of the array -> max one overflow
    if (pos >= sizeof(shc->m_buffer)/sizeof(shc->m_buffer[0]))
        pos -= sizeof(shc->m_buffer)/sizeof(shc->m_buffer[0]);

    return shc->m_buffer[pos];
}
